/*
 * calculation.cc
 *
 * Calculation tests
 *
 *  Created on: Oct 16, 2014
 *      Author: andy
 */

// Needed for M_PI, etc.
#define _USE_MATH_DEFINES
#include <algorithm>
#include <cstdlib>
#include <math.h>
#include <catch.hpp>
#include "../src/Radio.h"
#include "../src/calculation.h"

// Simple 2D vector struct, for computing with vectors.
struct vec2 {
	vec2(float xx, float yy) : x(xx), y(yy) { }

	float x,y;
};

/* sign(x)
 * Signum function: returns the sign of x as -1, 0, or 1.
 */
template <typename T> int sign(T val) {
    return (T(0) < val) - (val < T(0));
}

/* Perform an exhaustive *implementation* test of the calculation routine.
 * The test here is just a straightforward re-implementation of the
 * calculation routine, and thus functions only as the most basic test of
 * its correctness.
 */

// Wrap a value to [0..360), where the value may be negative.
inline int wrap(int v) {
	return ((v % 360) + 360) % 360;
}

TEST_CASE("Fixed tests of calculation", "[calculation]") {
	CalcResults results;

	calculate(0,45,true,results);
	CHECK(results.displacement == 45);

	calculate(0,135,true,results);
	CHECK(results.displacement == 45);
}

TEST_CASE("Exhaustive implementation test of calculation", "[calculation]") {
	CalcResults results;

	for(int sr = 0; sr < 360; ++sr)                   // Station (actual) radial
		for(int dr = 0; dr < 360; ++dr)   // Desired (input) radial
			for(int v = 0; v <= 1; ++v) {	          // Signal valid?
				calculate(sr, dr, v == 1, results);

				// ---- Check results ----

				CAPTURE(sr);
				CAPTURE(dr);

				// Make sure the results themselves are always valid
				CHECK(results.areResultsValid());

				if(!results.valid)
					continue; // Nothing to check

				if(results.direction == CalcResults::FROM) {
					// Normalize sr (so that dr is 0)
					int norm_sr = wrap(sr - dr);
					norm_sr = norm_sr > 180 ? norm_sr - 360 : norm_sr;

					CHECK(-norm_sr == results.displacement);
				}
				else if(results.direction == CalcResults::TO) {
					// Rotate by 180 degrees and check
					int norm_sr2 = wrap(180 + dr - sr);
					norm_sr2 = norm_sr2 > 180 ? norm_sr2 - 360 : norm_sr2;

					CHECK(-norm_sr2 == results.displacement);
				}
				else {
					int norm_sr = wrap(sr - dr);
					bool abreast = norm_sr == 90 || norm_sr == 270;
					CHECK(abreast);
				}
			}
}


void checkCalculationResults(int station_radial, int desired_radial, int valid,
		                     CalcResults &results) {

	// --- Test the results against the input ---

	// Convert radials from degrees to radians.
	float sr = 2 * M_PI * station_radial / 360.0;
	float dr = 2 * M_PI * desired_radial / 360.0;

	// Rotate so that dr is at 0
	sr -= dr;
	dr -= dr;

	// Convert to unit vectors. Note that this transformation looks wrong, because we are
	// converting to euclidean coordinates while at the same time rotating by 90 deg., so
	// that 0 degrees is <0,1>, *and* flipping the coordinate system so that +theta is
	// clockwise.
	vec2 sv(sin(sr),cos(sr));
	//vec2 dv(sin(dr),cos(dr));

	// Print the variables so we'll know what they are if things go wrong.
	// Inputs
	CAPTURE(station_radial);
	CAPTURE(desired_radial);
	CAPTURE(sr);
	CAPTURE(dr);
	CAPTURE(sv.x);
	CAPTURE(sv.y);
	CAPTURE(results.displacement);

	// Make sure the results themselves are valid. If this fails, the
	// entire test case will be aborted.
	REQUIRE(results.areResultsValid());

	// Check against results.
	if(results.valid) {

		// Due to rounding errors, sv.x may not be exactly 0 when it should be. So if
		// sv.x is "close enough" to 0 we round it off.
		sv.x = fabs(sv.x) < 0.0001 ? 0.0 : sv.x;

		if(results.direction == CalcResults::TO) {
			// Towards radial
			CHECK(sv.y < 0);
			CHECK(sign(sv.x) == -sign(results.displacement));

			// TODO: instead of/in addition to doing the following, turn
			// results.displacement into a vector and then compare its
			// position to that of sv.

			// Convert back to polar coordinates and check against displacement.
			// Again, note that while it may look like the arguments to atan2 are
			// in the wrong order, this is because we are operating in a transformed
			// coordinate system.
			// In addition, because this is the TO case, we have to rotate the radial
			// 180 degrees. TLDR: it looks wrong because math.
			float sva = round(360 * atan2(-sv.x,-sv.y) / (2 * M_PI));

			CHECK(sva == (float)results.displacement);
		}
		else if(results.direction == CalcResults::FROM) {
			// Away from radial
			CHECK(sv.y > 0);
			CHECK(sign(sv.x) == -sign(results.displacement));

			float sva = -round(360 * atan2(sv.x,sv.y) / (2 * M_PI));
			CHECK(sva == (float)results.displacement);
		}
		else if(results.direction == CalcResults::UNKNOWN) {
			// We can't check == 0.0, because of rounding errors, so we use a small epsilon.
			CHECK(sv.y < 0.0001);
			CHECK(sv.y > -0.0001);

			// Displacement/sv.x are irrelevant in this case.
		}
		else
			CHECK(false); // results.direction is not valid
	}
	// Else the signal is not valid because we are over the station. We
	// have nothing to check in that case, so we leave it be.

}



/* Perform an exhaustive test of the calculation routine. Note that we use
 * CHECK below instead of REQUIRE so that the test case will continue to
 * run after failures, allowing us to collect the entire set of failures.
 */
TEST_CASE("Exhaustive mathematical test of calculation", "[calculation]") {
	CalcResults results;

	for(int station_radial = 0; station_radial < 360; ++station_radial)
		for(int desired_radial = 0; desired_radial < 360; ++desired_radial)
			for(int v = 0; v <= 1; ++v) {
				// Do the calculation
				calculate(station_radial,desired_radial,v == 1,results);

				checkCalculationResults(station_radial,desired_radial,v == 1,results);

			}
}

TEST_CASE("Random test of calculation", "[calculation]") {
	CalcResults results;

	// We perform 360*360*2 random tests
	for(int i = 0; i < 360*360*2; ++i) {
		int station_radial = rand() % 400 - 20;
		int desired_radial = rand() % 400 - 20;
		int v = (rand() % 16) == 0;

		calculate(station_radial,desired_radial,v == 1,results);

		// We actually check against out-of-bounds radials here, which should generate invalid
		// calculation results.
		if(station_radial < 0 || station_radial >= 360 || desired_radial < 0 || desired_radial >= 360)
			CHECK(!results.valid);
		else
			checkCalculationResults(station_radial,desired_radial,v == 1,results);
	}

}
